# coding:utf-8
from PyQt5 import QtWidgets
from PyQt5.QtCore import *
from PyQt5.QtWidgets import *
import pandas as pd
import numpy as np
import sys
import os
import threading
import cv2
import sim
import re
import random
import pyrealsense2 as rs
import math
import model
import ControlBoard, detect_round, wdy_camera
from dobot_api import DobotApiDashboard, DobotApi, DobotApiMove

class MyWindow(QtWidgets.QMainWindow, ControlBoard.Ui_mainWindow):
    def __init__(self):
        super(MyWindow, self).__init__()
        threading.Thread.__init__(self)
        self.setupUi(self)
        self.ComputeMatrixClass=ComputeMatrixClass()
        self.VrepClass = CoppeliasimClass()  # 类的实例化
        self.jointhandles = []
        self.trans_end2hand = [45.96, 45.96, 267, 0.0000001, 0.0000001, 0.0000001]#末端到手，可以量出
        self.clientID = -1  # 默认关闭Vrep
        self.Connect.clicked.connect(lambda :self.ConnectAll())
        self.Disconnect.clicked.connect(lambda: self.DisconnectAll())
        # 笛卡尔按键控制
        self.Xplus.clicked.connect(lambda : self.Cartmove(1,'+'))
        iii=self.Xplus.isDown()
        self.Xplus_3.clicked.connect(lambda: self.Cartmove(3,'+'))
        self.Xplus_5.clicked.connect(lambda: self.Cartmove(5,'+'))
        self.Xplus_7.clicked.connect(lambda: self.Cartmove(7,'+'))
        self.Xplus_9.clicked.connect(lambda: self.Cartmove(9,'+'))
        self.Xplus_11.clicked.connect(lambda: self.Cartmove(11,'+'))
        self.Xplus_2.clicked.connect(lambda: self.Cartmove(2, '-'))
        self.Xplus_4.clicked.connect(lambda: self.Cartmove(4, '-'))
        self.Xplus_6.clicked.connect(lambda: self.Cartmove(6, '-'))
        self.Xplus_8.clicked.connect(lambda: self.Cartmove(8, '-'))
        self.Xplus_10.clicked.connect(lambda: self.Cartmove(10, '-'))
        self.Xplus_12.clicked.connect(lambda: self.Cartmove(12, '-'))
        # 关节空间按键控制
        self.Jplus_7.clicked.connect(lambda: self.Joinmove(7, '+'))
        self.Jplus_9.clicked.connect(lambda: self.Joinmove(9, '+'))
        self.Jplus_11.clicked.connect(lambda: self.Joinmove(11, '+'))
        self.Jplus_13.clicked.connect(lambda: self.Joinmove(13, '+'))
        self.Jplus_15.clicked.connect(lambda: self.Joinmove(15, '+'))
        self.Jplus_17.clicked.connect(lambda: self.Joinmove(17, '+'))
        self.Jplus_8.clicked.connect(lambda: self.Joinmove(8, '-'))
        self.Jplus_10.clicked.connect(lambda: self.Joinmove(10, '-'))
        self.Jplus_12.clicked.connect(lambda: self.Joinmove(12, '-'))
        self.Jplus_14.clicked.connect(lambda: self.Joinmove(14, '-'))
        self.Jplus_16.clicked.connect(lambda: self.Joinmove(16, '-'))
        self.Jplus_18.clicked.connect(lambda: self.Joinmove(18, '-'))
        self.Jplus_18.setAutoRepeat(True)
        self.FileButton.clicked.connect(lambda: self.OpenFile())
        self.StartMove.clicked.connect(lambda: self.ThreadFile())
        # 运动控制模块
        self.enable.clicked.connect(lambda: self.Enable())
        self.disable.clicked.connect(lambda : self.Disable())
        self.circledetect.clicked.connect(lambda : self.Detectcircle())
        self.boltdetect.clicked.connect(lambda : self.Detectbolt())
        self.circledetect_2.clicked.connect(lambda : self.Moveto())
        self.fullmission.clicked.connect(lambda : self.Fulldisasmble())
        self.speed.clicked.connect(lambda: self.confirmspeed())
    # 执行主要操作
    def ConnectAll(self):
        sim.simxFinish(-1)
        self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)  # 定义ID的类内变量
        if self.clientID != -1:
            self.VrepLabel.setStyleSheet('''QLabel{background-color: rgb(0, 234, 0)}''')  # Vrep指示灯亮
            self.LogText.append('Connect to Vrep')
            sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)  # 仿真初始化
            self.jointhandles = self.VrepClass.GetHandles(self.clientID, 6, 'UR5_joint')  # 得到六个关节角句柄
            self.ConnectRobot()
            if self.dashboard != False:
                self.VrepLabel_2.setStyleSheet('''QLabel{background-color: rgb(0, 234, 0)}''')  # robot指示灯亮
                self.LogText.append('Connect to Robot')
                self.dashboard.CP(99)
                self.dashboard.SpeedFactor(10)  # 默认速度为10
                self.ReadTimer = QTimer()  # 实例化系统定时器
                self.ReadTimer.timeout.connect(self.ReadTask)  # 定时执行函数   #####self.ReadTask
                self.ReadTimer.start(5)  # 定时时间5ms

                self.cam = self.ConnectCam()
                if self.cam.device_connected:
                    self.VrepLabel_3.setStyleSheet('''QLabel{background-color: rgb(0, 234, 0)}''')  # camera指示灯亮
                    self.LogText.append('Connect to camera')
                    self.hand2cam=[]  #读取标定结果
                    # 打开并读取txt文件
                    with open('C:/Users/22305/Desktop/Persen_pro/A_Learning/2024Learning/GraduationDesign/CodeData/Dobot/VisionModule/matlabprogram/end2cam5_8.txt', 'r') as cam2tool_file:
                        rota_data = cam2tool_file.readlines()
                    # 输出文件中的数据
                    for line in rota_data:
                        value = line.strip().split(',')  # 默认按空格分割
                        value = list(map(float, value))
                        value = np.array([num for num in value])
                        self.hand2cam.append(value)
                    self.hand2cam=np.array(self.hand2cam)
                    self.hand2cam[0:3,3]=self.hand2cam[0:3,3]*1000
                    cam2tool_file.close()
                    self.LogText.append('手眼矩阵加载完成')
                    self.CameraTimer = QTimer()  # 实例化系统定时器
                    self.CameraTimer.timeout.connect(self.camera_display)  # 定时执行函数   #####self.CamTask
                    self.CameraTimer.start(10)  # 定时时间5ms
                else:
                    self.LogText.append(' Failed connect to camera')
                    self.LogText.append("请检查重试")
            else:
                self.LogText.append(' Failed connect to Robot')
                self.LogText.append("请检查重试")
        else:
            self.LogText.append("Failed connect to Vrep")
            self.LogText.append("请检查重试")
    # 断联
    def DisconnectAll(self):
        self.dashboard.close()
        self.ReadTimer.stop()

        sim.simxFinish(-1)

        self.VrepLabel.setStyleSheet('')  # 指示灯灭
        self.VrepLabel_2.setStyleSheet('')  # 指示灯灭
        self.VrepLabel_3.setStyleSheet('')  # 指示灯灭
        self.cam.stop()
        self.CameraTimer.stop()
        cv2.destroyAllWindows()
        self.LogText.append('断开所有连接')
    # 笛卡尔空间运动，关节模式
    def Cartmove(self,num,signal):
        x = float(self.Xtext.toPlainText())
        y = float(self.Xtext_2.toPlainText())
        z = float(self.Xtext_3.toPlainText())
        rx = float(self.Xtext_4.toPlainText())
        ry = float(self.Xtext_5.toPlainText())
        rz = float(self.Xtext_6.toPlainText())
        if signal=='+':
            if num==1:
                x=x+0.6
            elif num ==3:
                y=y+0.6
            elif num==5:
                z+=0.6
            elif num==7:
                rx+=0.2
            elif num==9:
                ry+=0.2
            elif num ==11:
                rz+=0.2
        elif signal=='-':
            if num==2:
                x=x-0.2
            elif num ==4:
                y=y-0.2
            elif num==6:
                z-=0.2
            elif num==8:
                rx-=0.2
            elif num==10:
                ry-=0.2
            elif num ==12:
                rz-=0.2
        inverjoint=self.dashboard.InverseSolution(x,y,z,rx,ry,rz,0,0)
        # 使用正则表达式提取第一个大括号内的数据
        match = re.search(r'\{(.*?)\}', inverjoint)
        if match:
            joint=match.group(1)
            jpose=joint.strip().split(',')
            lastpose = np.array(list(map(float, jpose))) * np.pi / 180
            for i in range(6):
                sim.simxSetJointTargetPosition(self.clientID,self.jointhandles[i],lastpose[i],sim.simx_opmode_blocking)
        self.move_api.MovJ(x,y,z,rx,ry,rz,0,0)
    #  关节空间运动
    def Joinmove(self,num,signal):
        j1=float(self.J1text.toPlainText())
        j2 = float(self.J1text_2.toPlainText())
        j3 = float(self.J1text_3.toPlainText())
        j4 = float(self.J1text_4.toPlainText())
        j5 = float(self.J1text_5.toPlainText())
        j6 = float(self.J1text_6.toPlainText())
        if signal=='+':
            if num==7:
                j1+=0.2
            elif num==9:
                j2+=0.2
            elif num==11:
                j3+=0.2
            elif num==13:
                j4+=0.2
            elif num==15:
                j5+=0.2
            elif num==17:
                j6+=0.2
        elif signal=='-':
            if num==8:
                j1-=0.2
            elif num==10:
                j2-=0.2
            elif num==12:
                j3-=0.2
            elif num==14:
                j4-=0.2
            elif num==16:
                j5-=0.2
            elif num==18:
                j6-=0.2
        jpose=np.array([j1,j2,j3,j4,j5,j6])
        for i in range(6):
            sim.simxSetJointTargetPosition(self.clientID,self.jointhandles[i],jpose[i],sim.simx_opmode_blocking)
        self.move_api.JointMovJ(j1, j2,j3, j4, j5, j6, 0, 0)
    # 打开路径文件
    def OpenFile(self):
        filename, _ = QFileDialog.getOpenFileName(self, '请选择文件', '', 'CSV(*.csv)')
        self.interpolationpath = pd.read_csv(filename)  # 路径变量，便于全局使用
        self.interpolationpath = self.interpolationpath.to_numpy()
    # 开启另一个线程解决耗时任务
    def ThreadFile(self):
        task1 = threading.Thread(target=self.StartPath)  # 这里的StartPath不能加括号，要不开启不了线程
        task1.setDaemon(True)
        task1.start()
    # 路径开始，笛卡尔空间，关节模式
    def StartPath(self):
        for point in self.interpolationpath:
            self.move_api.MovJ(point[0],point[1],point[2],point[3],point[4],point[5],0,0)
            # 提取角度，传给仿真平台
            angle=self.dashboard.GetAngle()
            # 使用正则表达式提取第一个大括号内的数据
            match = re.search(r'\{(.*?)\}', angle)
            if match:
                zhengjie = match.group(1)
                jpose = zhengjie.strip().split(',')
                rota = np.array(list(map(float, jpose))) * np.pi / 180
            for i in range(6):
                sim.simxSetJointTargetPosition(self.clientID, self.jointhandles[i], rota[i], sim.simx_opmode_blocking)
    # 机器人使能
    def Enable(self):
        print("开始使能...")
        try:
            self.dashboard.EnableRobot()  # enablerobot
        except Exception as e:
            print("使能失败")
            sys.exit(0)
        print("使能成功:)")
        self.LogText.append('机器人使能成功')
    # 机器人失能
    def Disable(self):
        try:
            self.dashboard.DisableRobot()
        except Exception as e:
            sys.exit(0)
        self.LogText.append('机器人下使能成功')
    # 识别孔洞
    def Detectcircle(self):
        """检测图像中的圆形或椭圆孔洞，并获取其中心在相机坐标系下的3D位置"""
        color_img, depth_img = self.cam.get_images()
        if color_img is None or depth_img is None:
            self.LogText.append("无法加载图像，请检查相机连接。")
            return

        # 获取相机内参
        intrinsics = self.cam.get_intrinsics()
        if intrinsics is None:
            self.LogText.append("无法获取相机内参。")
        else:
            fx, fy, cx, cy = intrinsics

        try:
            self.LogText.append('正在使用OpenCV检测圆形或椭圆孔洞')
            original_color = color_img.copy()

            # 预处理
            kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
            morph_image = cv2.morphologyEx(original_color, cv2.MORPH_OPEN, kernel)
            gray = cv2.cvtColor(morph_image, cv2.COLOR_BGR2GRAY)
            binary = cv2.adaptiveThreshold(gray, 255,
                                        cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
                                        cv2.THRESH_BINARY,
                                        11, 5)
            edges = cv2.Canny(binary, 20, 200, apertureSize=3, L2gradient=True)

            contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

            # 存储最终有效的椭圆和3D位置
            valid_ellipses_with_3d = []  # [(ellipse, [X,Y,Z]), ...]

            for contour in contours:
                area = cv2.contourArea(contour)
                arc_len = cv2.arcLength(contour, closed=True)
                if area < 100 or arc_len < 30:
                    continue
                if len(contour) >= 5:
                    try:
                        ellipse = cv2.fitEllipse(contour)
                        (x, y), (a, b), angle = ellipse
                        ratio = a / b if a > b else b / a
                        if not (1.0 <= ratio <= 2.0):
                            continue

                        x_int, y_int = int(x), int(y)
                        h, w = depth_img.shape[:2]
                        if not (0 <= x_int < w and 0 <= y_int < h):
                            continue

                        depth_val = depth_img[y_int, x_int]
                        if depth_val <= 0:
                            continue  # 跳过无效深度

                        # 计算3D坐标（单位：mm）
                        X = (x_int - cx) * depth_val / fx
                        Y = (y_int - cy) * depth_val / fy
                        Z = depth_val
                        valid_ellipses_with_3d.append((ellipse, [X, Y, Z]))

                    except Exception as e:
                        continue  # 拟合失败则跳过

            # === 绘制最终保留的孔洞 ===
            output_img = original_color.copy()
            hole_3d_positions = []

            for idx, (ellipse, pos3d) in enumerate(valid_ellipses_with_3d):
                center = tuple(map(int, ellipse[0])) 
                axes = tuple(map(int, ellipse[1]))
                angle = ellipse[2]

                # 绘制椭圆（绿色）
                cv2.ellipse(output_img, center, axes, angle, 0, 360, (0, 255, 0), 2)

                # 添加编号标签
                label = f"{idx + 1}"
                font = cv2.FONT_HERSHEY_SIMPLEX
                font_scale = 0.6
                thickness = 2
                text_size, _ = cv2.getTextSize(label, font, font_scale, thickness)
                cv2.rectangle(output_img,
                            (center[0] - 10, center[1] - 25),
                            (center[0] - 10 + text_size[0] + 5, center[1]),
                            (0, 255, 0), -1)
                cv2.putText(output_img, label, (center[0] - 8, center[1] - 8),
                            font, font_scale, (255, 255, 255), thickness)

                hole_3d_positions.append(pos3d)

            # 显示图像（使用 OpenCV 显示，也可以改为 QLabel 显示）
            cv2.imshow("Circle_detect", output_img)
            cv2.waitKey(2000)
            cv2.destroyAllWindows()

            # 存储3D位置
            self.targetpoint = np.array(hole_3d_positions) if hole_3d_positions else np.empty((0, 3))
            self.LogText.append('孔洞检测已完成，3D位置（相机坐标系，单位mm）：')
            self.LogText.append('self.targetpoint : ')
            self.LogText.append(str(self.targetpoint))
        except Exception as e:
            self.LogText.append("检测圆形或椭圆孔洞失败:"+str(e))
    # 识别螺栓
    def Detectbolt(self):
        """使用yolo训练的模型来识别螺栓"""
        import onnxruntime as rt
        # 识别螺栓
        height, width = 640, 640
        color_img, depth_img = self.cam.get_images()
        intrinsics = self.cam.get_intrinsics()
        if intrinsics is None:
            self.LogText.append("无法获取相机内参")
            return
        fx, fy, cx, cy = intrinsics

        try:
            self.LogText.append('正在使用YOLO模型识别螺栓')
            img0 = color_img.copy()
            x_scale = img0.shape[1] / width
            y_scale = img0.shape[0] / height
            # preprocess the image
            img = img0 / 255.
            img = cv2.resize(img, (width, height))
            img = np.transpose(img, (2, 0, 1))
            data = np.expand_dims(img, axis=0)
            # ONNX Runtime
            sess = rt.InferenceSession("Dobot/visionm_1/best1.onnx")
            input_name = sess.get_inputs()[0].name
            label_name = sess.get_outputs()[0].name
            pred = sess.run([label_name], {input_name: data.astype(np.float32)})[0]
            pred = np.squeeze(pred)
            pred = np.transpose(pred, (1, 0))
            pred_class = pred[..., 4:]
            pred_conf = np.max(pred_class, axis=-1)
            pred = np.insert(pred, 4, pred_conf, axis=-1)
            result = model.nms(pred, 0.4, 0.3)
            self.LogText.append(f"识别结果：{result}")

            # 从检测结果中提取螺栓中心的3D坐标
            pose_bolt_list = [] # [[X,Y,Z], ...]
            valid_result = [] # 存储有效的检测结果  

            if result is not None and len(result) > 0:
                for det in result:
                    # det 格式假设为 [x1, y1, x2, y2, conf, cls]
                    x1, y1, x2, y2 = det[:4]
                    # 还原到原始图像尺寸
                    x1_orig = int(x1 * x_scale)
                    y1_orig = int(y1 * y_scale)
                    x2_orig = int(x2 * x_scale)
                    y2_orig = int(y2 * y_scale)
                    # 计算中心点（2D）
                    cx_2d = int((x1_orig + x2_orig) / 2)
                    cy_2d = int((y1_orig + y2_orig) / 2)

                    # 获取该点的深度值（单位：毫米）
                    # 注意：depth_img 通常是 16-bit，单位为毫米（RealSense 默认）
                    if 0 <= cx_2d < depth_img.shape[1] and 0 <= cy_2d < depth_img.shape[0]:
                        depth_val = depth_img[cy_2d, cx_2d]  # 单位：mm
                        if depth_val > 0:  # 有效深度
                            # 使用针孔相机模型反投影
                            X = (cx_2d - cx) * depth_val / fx
                            Y = (cy_2d - cy) * depth_val / fy
                            Z = depth_val
                            pose_bolt_list.append([X, Y, Z])  # 相机坐标系下 (mm)
                            valid_result.append(det)

                        else:
                            self.LogText.append(f"深度无效，跳过目标：({cx_2d}, {cy_2d})")
                    else:
                        self.LogText.append(f"坐标越界：({cx_2d}, {cy_2d})")

            # 绘制最终保留的螺栓
            ret_img, _ = model.draw(img0, x_scale, y_scale, valid_result)

            cv2.imshow("Bolt_detect",ret_img)
            cv2.waitKey(2000)
            cv2.destroyAllWindows()
            # -----获得螺栓中心相对于相机的中心坐标并存储,转变为类变量
            self.targetpoint=np.array(pose_bolt_list)
            self.LogText.append('螺栓检测已完成：')
            self.LogText.append(str(self.targetpoint))
        except Exception as e:
            self.LogText.append("识别螺栓失败:"+str(e))
    #  运动至
    def Moveto(self):
        """将相机获取到的空间点坐标转换到机器人基座标下，并运动至"""
        try:
            position_in_robot=self.cam2base(point=self.targetpoint)
            self.dashboard.SpeedFactor(10)
            self.LogText.append('运动速率为：10')
            posi_in_robot=np.array([position_in_robot])
            print('postion',posi_in_robot)
            self.LogText.append(str(posi_in_robot))
            self.move_api.MovJ(posi_in_robot[0,0],posi_in_robot[0,1],posi_in_robot[0,2],posi_in_robot[0,3],posi_in_robot[0,4],
                            posi_in_robot[0,5],0,0)
            self.LogText.append(f'笛卡尔空间关节运动：{str(posi_in_robot)}')
        except Exception as e:
            self.LogText.append("运动失败:"+str(e))

    # 将相机获取到的空间点坐标转换到机器人基座标下
    def cam2base(self,point):
        """将相机获取到的空间点坐标转换到机器人基座标下"""
        try:
            point=point[0,:]
            point=np.hstack((point,[0,0,0]))
            print('物体在相机下位姿:', point)
            robotpose=self.dashboard.GetPose() # 获取末端到基坐标系的变换矩阵
            # 使用正则表达式提取第一个大括号内的数据
            match = re.search(r'\{(.*?)\}', robotpose)
            if match:
                robot = match.group(1)
                jpose = robot.strip().split(',')
                end2base = np.array(list(map(float, jpose)))
                # end2base= self.ComputeMatrixClass.pose_to_matrix(end2base)
                print('end2base:', end2base)
            hand2basemt = self.ComputeMatrixClass.multiply_transforms(end2base, self.trans_end2hand) #手在基座标的位姿
            hand2base = self.ComputeMatrixClass.matrix_to_pose(hand2basemt)
            print('hand2base:', hand2base)
            cameramt=self.ComputeMatrixClass.multiply_transforms(hand2base, self.hand2cam) # 相机在基座标的位姿
            camerapose=self.ComputeMatrixClass.matrix_to_pose(cameramt)
            print('camera2base:',camerapose)
            point2basemt = self.ComputeMatrixClass.multiply_transforms(camerapose, point)# 螺栓在基座标的位姿
            point = self.ComputeMatrixClass.matrix_to_pose(point2basemt)
            print('Bolt2base:',  point)
            point = point - hand2base + end2base
        except Exception as e:
            self.LogText.append("转换失败:"+str(e))

        return point
    
    def Fulldisasmble(self):
        pass
    # 速度确认
    def confirmspeed(self):
        self.fullspeed = int(self.speedtext.toPlainText())
        self.dashboard.SpeedFactor(self.fullspeed)
        self.LogText.append(f'全局速度因子为：{str(self.fullspeed)}')
    # 读取数据
    def ReadTask(self):
        pose=self.dashboard.GetPose()
        # 使用正则表达式提取第一个大括号内的数据
        cmatch = re.search(r'\{(.*?)\}', pose)
        if cmatch:
            zhengjie = cmatch.group(1)
            cpose = zhengjie.strip().split(',')
            # cpose=[format(num, '.3f') for num in cpose]
            self.Xtext.setText(cpose[0])
            self.Xtext_2.setText(cpose[1])
            self.Xtext_3.setText(cpose[2])
            self.Xtext_4.setText(cpose[3])
            self.Xtext_5.setText(cpose[4])
            self.Xtext_6.setText(cpose[5])

        angle=self.dashboard.GetAngle()
        # 使用正则表达式提取第一个大括号内的数据
        jmatch = re.search(r'\{(.*?)\}', angle)
        if jmatch:
            zheng = jmatch.group(1)
            jpose = zheng.strip().split(',')
            self.J1text.setText(jpose[0])
            self.J1text_2.setText(jpose[1])
            self.J1text_3.setText(jpose[2])
            self.J1text_4.setText(jpose[3])
            self.J1text_5.setText(jpose[4])
            self.J1text_6.setText(jpose[5])
    # tcp指令返回值处理函数，返回值为字符串里包含数字的数组
    def parseResultId(valueRecv):
        if valueRecv.find("Not Tcp") != -1:  # 通过返回值判断机器是否处于tcp模式
            print("Control Mode Is Not Tcp")
            return [-2]
        commandArrID = re.findall(r'-?\d+', valueRecv)
        # print("commandArrID", commandArrID)
        commandArrID = [int(num) for num in commandArrID]
        if commandArrID[0] == 0:
            if len(commandArrID) >= 2:
                return commandArrID  # 获取运动指令的commandArrID
            return [-1]
        else:
            # 根据返回值来判断机器处于什么状态
            if commandArrID[0] == -1:
                print("Queue command exceeds queue depth 64")
            elif commandArrID[0] == -2:
                print("The robot is in an error state")
            elif commandArrID[0] == -3:
                print("The robot is in emergency stop state")
            elif commandArrID[0] == -4:
                print("The robot is in power down state")
            else:
                print("The robot is in Uknown state", commandArrID[0])
        return [-2]
    # 机器tcp连接函数
    def ConnectRobot(self):
        try:
            ip = "192.168.5.1"
            dashboardPort = 29999
            movePort = 30003
            feedPort = 30004
            print("正在建立连接...")
            self.dashboard = DobotApiDashboard(ip, dashboardPort)
            self.move_api = DobotApiMove(ip, movePort)
            self.feed = DobotApi(ip, feedPort)
            self.dashboard.ClearError()  # 清除错误
            print(">.<连接成功>!<")
        except Exception as e:
            print(":(连接失败:(")
            raise e
    # 连接相机
    def ConnectCam(self):
        cam=wdy_camera.RealSenseThread()
        return cam

    def camera_display(self):
        try:
            color_frame, _ = self.cam.get_images()
            if color_frame is not None:
                cv2.imshow("RealSense", color_frame)
                cv2.waitKey(1)
        except Exception as e:
            print("Error displaying camera feed:", e)


class CoppeliasimClass():
    def GetHandles(self, client, number, name):
        objecthandle = []
        if number < 2:
            _, objecthandle = sim.simxGetObjectHandle(client, name, sim.simx_opmode_blocking)
            return objecthandle
        else:
            for i in range(number):
                _, Handle = sim.simxGetObjectHandle(client, name + str(i + 1), sim.simx_opmode_blocking)
                objecthandle.append(Handle)
            return objecthandle


class ComputeMatrixClass():
    # 欧拉角转为矩阵
    def angle2rotation(self,x, y, z):
        Rx = np.array([[1, 0, 0], [0, math.cos(x), -math.sin(x)], [0, math.sin(x), math.cos(x)]])
        Ry = np.array([[math.cos(y), 0, math.sin(y)], [0, 1, 0], [-math.sin(y), 0, math.cos(y)]])
        Rz = np.array([[math.cos(z), -math.sin(z), 0], [math.sin(z), math.cos(z), 0], [0, 0, 1]])
        R = Rz @ Ry @ Rx
        return R

    # 矩阵转为欧拉角
    def rotation2euler_angles(self,R):
        sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
        singular = sy < 1e-6
        if not singular:
            x = math.atan2(R[2, 1], R[2, 2])
            y = math.atan2(-R[2, 0], sy)
            z = math.atan2(R[1, 0], R[0, 0])
        else:
            x = math.atan2(-R[1, 2], R[1, 1])
            y = math.atan2(-R[2, 0], sy)
            z = 0
        return np.array([x, y, z])
    def pose_to_matrix(self,pose): # 传进来的是数组才行
        """
        Convert pose information [x, y, z, rx, ry, rz] to a 4x4 transformation matrix.
        """
        pose=np.array([pose])
        x, y, z, rx, ry, rz = pose[0,0],pose[0,1],pose[0,2],pose[0,3],pose[0,4],pose[0,5]
        translation = np.array([[x], [y], [z]])
        rotation = self.angle2rotation(rx, ry, rz)[0]
        matrix = np.vstack((np.hstack((rotation, translation)), [0, 0, 0, 1]))
        return matrix
    def matrix_to_pose(self,matrix):
        """
        Convert a 4x4 transformation matrix to pose information [x, y, z, rx, ry, rz].
        """
        translation = matrix[:3, 3]
        rotation = matrix[:3, :3]
        R_vect=self.rotation2euler_angles(rotation)
        pose = np.array([translation[0], translation[1], translation[2], R_vect[0,0],R_vect[1,0],R_vect[2,0]])
        return pose
    def multiply_transforms(self,ini_pose, trans_pose): # 传进来一个向量和一个矩阵
        """
        Multiply two transformation matrices obtained from initial and transformation poses.
        """
        T_ini = self.pose_to_matrix(ini_pose)
        T_trans = self.pose_to_matrix(trans_pose)
        T_final = np.dot(T_ini, T_trans)
        return T_final


if __name__=='__main__':
    app = QtWidgets.QApplication(sys.argv)
    window = MyWindow()
    window.show()
    sys.exit(app.exec_())